#include "followingPath.h"

int global_state;

ros::Publisher moto_pub;
ros::Publisher moto_change_pub;

saint_nr_9::IRdata ir_data;

void ir_callback(const saint_nr_9::IRdata::ConstPtr& data){
	ir_data = *data;
}


void state_straight()
{
  //printf("State_Straight\n");
  saint_nr_9::MotCom moto_command;
  if (fabs(ir_data.white - ir_data.orange) < DIFFERENCE_IS_WALL && ir_data.white < DISTANCE_IS_WALL && ir_data.orange < DISTANCE_IS_WALL)
  {
    moto_command.command = MOVE_FORWARD_CORRECT;
    moto_command.speed = SPEED_WALL;
    saint_nr_9::MotCorrect correct;
    correct.left = 0;
    correct.right = 0;
    
    double CORRECTION_SLOWDOWN = 0.5; //try 0.3
    double front = (ir_data.white < 0? 0:ir_data.white);
    double back = (ir_data.orange < 0? 0:ir_data.orange);
    if (front > back){
      correct.right = 1 - (back / front);
      correct.right *= CORRECTION_SLOWDOWN;
    }else{
      correct.left = 1 - (front / back);
      correct.left *= CORRECTION_SLOWDOWN;
    }  
    moto_change_pub.publish(correct);
  }
  else if(fabs(ir_data.green - ir_data.black) < DIFFERENCE_IS_WALL && ir_data.green < DISTANCE_IS_WALL && ir_data.black < DISTANCE_IS_WALL)
  {
    moto_command.command = MOVE_FORWARD_CORRECT;
    moto_command.speed = SPEED_WALL;
    saint_nr_9::MotCorrect correct;
    correct.left = 0;
    correct.right = 0;
    
    double CORRECTION_SLOWDOWN = 0.5; //try 0.3
    double front = (ir_data.green < 0? 0:ir_data.green);
    double back = (ir_data.black < 0? 0:ir_data.black);
    if (front > back){
      correct.left = 1 - (back / front);
      correct.left *= CORRECTION_SLOWDOWN;
    }else{
      correct.right = 1 - (front / back);
      correct.right *= CORRECTION_SLOWDOWN;
    }  
    moto_change_pub.publish(correct); 
  }
  else
  {
    moto_command.command = MOVE_FORWARD;
    moto_command.speed = SPEED_SLOW;
  }
  moto_pub.publish(moto_command);
}

void state_left()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_LEFT;
  moto_command.dist_or_angle = 90;
  moto_command.speed = SPEED_TURN;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_right()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_RIGHT;
  moto_command.dist_or_angle = 90;
  moto_command.speed = SPEED_TURN;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_u_turn()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_RIGHT;
  moto_command.dist_or_angle = 180;
  moto_command.speed = SPEED_TURN;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}



void search_tag()
{
  
}

int main(int argc, char **argv){
	ros::init(argc, argv, "PathFollower"); //Creates a node named "PathFollower"
	ros::NodeHandle n;
	ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_update);
	moto_pub = n.advertise<saint_nr_9::MotCom>("/control/motor_control", 1);
	moto_change_pub = n.advertise<saint_nr_9::MotCorrect>("/control/correct_motor",1);
	 //ros::Subscriber state_sub = n.subscribe("state_info",1, );
	ros::Subscriber ir_sub = n.subscribe("ir_info", 1, ir_callback);
    ros::Rate loop_rate(100);
  printf("PathFollower is running \n");
  
  while(ros::ok())
  {
    if(global_state == PATH_STRAIGHT)
    {
      state_straight();

    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}


void state_update(const std_msgs::Int32::ConstPtr& state)
{
  global_state = state->data;
  printf("Input state: %d\n", state->data);
  if(global_state == PATH_STRAIGHT) state_straight();
  else if(global_state == PATH_LEFT_TURN) state_left();
  else if(global_state == PATH_RIGHT_TURN) state_right();
  else if(global_state == PATH_U_TURN) state_u_turn();
  else if(global_state == PATH_SEARCH_TAG) search_tag();
  else 
  {
    global_state = -1;
  }
}
